# 多策略避障检测
import  cv2
import numpy as np
import time

class ObstacleDetector:
    def __init__(self):
        # 初始化背景减法器用于运动检测
        self.fgbg = cv2.createBackgroundSubtractorMOG2(history=50, varThreshold=16, detectShadows=False)
        
        # 初始化光流参数
        self.prev_gray = None
        self.feature_params = dict(maxCorners=100, qualityLevel=0.3, minDistance=7, blockSize=7)
        self.lk_params = dict(winSize=(15, 15), maxLevel=2, 
                            criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 10, 0.03))
        
        # 障碍物检测参数
        self.obstacle_threshold = 0.25  # 障碍物占据画面的比例阈值
        self.safe_distance_ratio = 0.6  # 安全距离占画面高度的比例
    
    def detect_obstacles(self, frame):
        # 多策略检测结果
        results = {
            'edge_obstacle': False,
            'motion_obstacle': False,
            'depth_obstacle': False,
            'combined_obstacle': False
        }
        
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        h, w = gray.shape
        
        # 1. 基于边缘的障碍物检测
        edges = cv2.Canny(gray, 50, 150)
        roi_edges = edges[int(h*0.6):h, :]  # 只检测图像下半部分
        
        # 计算边缘密度
        edge_density = np.sum(roi_edges > 0) / (roi_edges.size + 1e-6)
        results['edge_obstacle'] = edge_density > 0.1
        
        # 2. 基于运动的障碍物检测
        fgmask = self.fgbg.apply(frame)
        fgmask = cv2.erode(fgmask, None, iterations=1)
        fgmask = cv2.dilate(fgmask, None, iterations=2)
        
        motion_ratio = np.sum(fgmask > 0) / (h * w)
        results['motion_obstacle'] = motion_ratio > 0.05
        
        # 3. 基于"深度"的障碍物检测(伪深度，基于物体大小和位置)
        # 检测图像下半部分的较大轮廓
        _, thresh = cv2.threshold(gray[int(h*0.5):, :], 0, 255, cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
        contours, _ = cv2.findContours(thresh, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        
        obstacle_area = 0
        for cnt in contours:
            area = cv2.contourArea(cnt)
            if area > 500:  # 只考虑较大的轮廓
                obstacle_area += area
        
        obstacle_ratio = obstacle_area / (w * h * 0.5)  # 下半部分区域
        results['depth_obstacle'] = obstacle_ratio > self.obstacle_threshold
        
        # 综合判断
        results['combined_obstacle'] = (results['edge_obstacle'] or 
                                      results['motion_obstacle'] or 
                                      results['depth_obstacle'])
        
        return results
    
    def get_safe_direction(self, frame):
        gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
        h, w = gray.shape
        
        # 分析左右两侧的边缘密度
        left_region = gray[int(h*0.6):, :w//2]
        right_region = gray[int(h*0.6):, w//2:]
        
        left_edges = cv2.Canny(left_region, 50, 150)
        right_edges = cv2.Canny(right_region, 50, 150)
        
        left_density = np.sum(left_edges > 0) / (left_edges.size + 1e-6)
        right_density = np.sum(right_edges > 0) / (right_edges.size + 1e-6)
        
        # 返回更安全的方向 (边缘密度低的一侧)
        if left_density < right_density:
            return 'left'
        else:
            return 'right'
        

    def process_frame(self, frame):
        # 障碍物检测
        obstacle_results = self.detect_obstacles(frame)
        
        # 获取安全方向
        safe_direction = self.get_safe_direction(frame)
        
        # 控制决策
        if obstacle_results['combined_obstacle']:
            if safe_direction == 'left':
                action = "Turning Left"
            else:
                action = "Turning Right"
            time.sleep(0.5)  # 转向持续时间
        else:
            action = "Moving Forward"
        
        font=cv2.FONT_HERSHEY_SIMPLEX
        cv2.putText(frame,  action,
                            (500, 50), font,1,(0, 255, 255),2,cv2.LINE_4)
        return frame        